
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_gps.c
  * @author     baiyang
  * @date       2022-2-16
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <assert.h>
#include <limits.h>
#include <float.h>
#include <string.h>

#include "sensor_gps.h"

#include "sensor_gps_ublox.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#include <rtdebug.h>
#include <rtc/gp_rtc.h>
#include <notify/notify.h>
#include <mavproxy/mavproxy.h>
#include <common/time/gp_time.h>
#include <common/console/console.h>
#include <serial_manager/gp_serial.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#define GPS_RTK_INJECT_TO_ALL 127
#ifndef GPS_MAX_RATE_MS
#define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms
#endif
#define GPS_BAUD_TIME_MS 1200
#define GPS_TIMEOUT_MS 4000u

// defines used to specify the mask position for use of different accuracy metrics in the blending algorithm
#define BLEND_MASK_USE_HPOS_ACC     1
#define BLEND_MASK_USE_VPOS_ACC     2
#define BLEND_MASK_USE_SPD_ACC      4
#define BLEND_COUNTER_FAILURE_INCREMENT 10

#ifndef HAL_GPS_COM_PORT_DEFAULT
#define HAL_GPS_COM_PORT_DEFAULT 1
#endif

#define GPS_DEBUGGING 0

#if GPS_DEBUGGING
#define GPS_DEUBG(fmt, args ...)  do {console_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else
#define GPS_DEUBG(fmt, args ...)
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool needs_uart(enum GPS_Type type);
#if defined(GPS_BLENDED_INSTANCE)
static bool calc_blend_weights(void);
static void calc_blended_state(void);
#endif
static uint16_t get_rate_ms(uint8_t instance);
static void publish_sensor_gps();
/*----------------------------------variable----------------------------------*/
// baudrates to try to detect GPSes with
static const uint32_t _baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U, 460800U};

// initialisation blobs to send to the GPS to try to get it into the
// right mode
static const char _initialisation_blob[] = UBLOX_SET_BINARY_230400;

sensor_gps gps;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_gps_ctor()
{
    if (!((sizeof(_initialisation_blob) * (CHAR_BIT + 2)) < (4800 * GPS_BAUD_TIME_MS * 1e-3))) {
        rt_assert_handler("GPS initilisation blob is too large to be completely sent before the baud rate changes", __FUNCTION__, __LINE__);
    }
    sensor_gps_assign_param();

    gps._baudrates = &_baudrates[0];
    gps._initialisation_blob = &_initialisation_blob[0];
}

/// Startup initialisation.
void sensor_gps_init()
{
    // search for serial ports with gps protocol
    uint8_t uart_idx = 0;
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        if (needs_uart((enum GPS_Type)gps._type[i])) {
            gps._port[i] = SerialManager_find_protocol_instance(SerialProtocol_GPS, uart_idx);
            uart_idx++;
        }
    }
    gps._last_instance_swap_ms = 0;

    // Initialise class variables used to do GPS blending
    gps._omega_lpf = 1.0f / math_constrain_float(gps._blend_tc, 5.0f, 30.0f);

    // prep the state instance fields
    for (uint8_t i = 0; i < GPS_MAX_INSTANCES; i++) {
        gps.state[i].instance = i;
    }

    // sanity check update rate
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        if (gps._rate_ms[i] <= 0 || gps._rate_ms[i] > GPS_MAX_RATE_MS) {
            gps._rate_ms[i] = GPS_MAX_RATE_MS;
        }
    }
}

/*
  returns the desired gps update rate in milliseconds
  this does not provide any guarantee that the GPS is updating at the requested
  rate it is simply a helper for use in the backends for determining what rate
  they should be configuring the GPS to run at
*/
uint16_t sensor_gps_get_rate_ms(uint8_t instance)
{
    // sanity check
    if (instance >= gps.num_instances || gps._rate_ms[instance] <= 0) {
        return GPS_MAX_RATE_MS;
    }
    return MIN(gps._rate_ms[instance], GPS_MAX_RATE_MS);
}

/*
  return the expected lag (in seconds) in the position and velocity readings from the gps
  return true if the GPS hardware configuration is known or the delay parameter has been set
 */
bool sensor_gps_get_lag(uint8_t instance, float *lag_sec)
{
    // always enusre a lag is provided
    *lag_sec = 0.22f;

    if (instance >= GPS_MAX_INSTANCES) {
        return false;
    }

#if defined(GPS_BLENDED_INSTANCE)
    // return lag of blended GPS
    if (instance == GPS_BLENDED_INSTANCE) {
        *lag_sec = gps._blended_lag_sec;
        // auto switching uses all GPS receivers, so all must be configured
        uint8_t inst; // we don't actually care what the number is, but must pass it
        return sensor_gps_first_unconfigured_gps(&inst);
    }
#endif

    if (gps._delay_ms[instance] > 0) {
        // if the user has specified a non zero time delay, always return that value
        *lag_sec = 0.001f * (float)gps._delay_ms[instance];
        // the user is always right !!
        return true;
    } else if (gps.drivers[instance] == NULL || gps.state[instance].status == NO_GPS) {
        // no GPS was detected in this instance so return the worst possible lag term
        if (gps._type[instance] == GPS_TYPE_NONE) {
            *lag_sec = 0.0f;
            return true;
        }
        return gps._type[instance] == GPS_TYPE_AUTO;
    } else {
        // the user has not specified a delay so we determine it from the GPS type
        return sensor_gps_backend_get_lag(gps.drivers[instance], lag_sec);
    }
}

bool sensor_gps_first_unconfigured_gps(uint8_t *instance)
{
    for (int i = 0; i < GPS_MAX_RECEIVERS; i++) {
        if (gps._type[i] != GPS_TYPE_NONE && (gps.drivers[i] == NULL || !sensor_gps_backend_is_configured(gps.drivers[i]))) {
            *instance = i;
            return true;
        }
    }
    return false;
}

// get configured type by instance
enum GPS_Type sensor_gps_get_type(uint8_t instance)
{
    return instance>=GPS_MAX_RECEIVERS? GPS_TYPE_NONE : (enum GPS_Type)(gps._type[instance]);
}

/*
  send some more initialisation string bytes if there is room in the
  UART transmit buffer
 */
void sensor_gps_send_blob_start(uint8_t instance, const char *_blob, uint16_t size)
{
    gps.initblob_state[instance].blob = _blob;
    gps.initblob_state[instance].remaining = size;
}

/*
  send some more initialisation string bytes if there is room in the
  UART transmit buffer
 */
void sensor_gps_send_blob_update(uint8_t instance)
{
    // exit immediately if no uart for this instance
    if (gps._port[instance] == NULL) {
        return;
    }

    // see if we can write some more of the initialisation blob
    if (gps.initblob_state[instance].remaining > 0) {
        int16_t space = SerialManager_tx_space(gps._port[instance]);
        if (space > (int16_t)gps.initblob_state[instance].remaining) {
            space = gps.initblob_state[instance].remaining;
        }
        while (space > 0) {
            SerialManager_write_byte(gps._port[instance], *gps.initblob_state[instance].blob);
            gps.initblob_state[instance].blob++;
            space--;
            gps.initblob_state[instance].remaining--;
        }
    }
}

/*
  update all GPS instances
 */
void sensor_gps_update(void)
{
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        sensor_gps_update_instance(i);
    }

    // calculate number of instances
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        if (gps.drivers[i] != NULL) {
            gps.num_instances = i+1;
        }
    }

#if GPS_MAX_RECEIVERS > 1
    sensor_gps_update_primary();
#endif  // GPS_MAX_RECEIVERS > 1

    // update notify with gps status. We always base this on the primary_instance
    notify_flags.gps_status = gps.state[gps.primary_instance].status;
    notify_flags.gps_num_sats = gps.state[gps.primary_instance].num_sats;

    publish_sensor_gps();

#if GPS_DEBUGGING
    for (uint8_t i = 0; i < sensor_gps_num_sensors(); i++) {
        //GPS update rate acceptable
        if (!sensor_gps_is_healthy(i)) {
            GPS_DEUBG("GPS %i: not healthy", i+1);
        }
    }
#endif
}

/*
  update primary GPS instance
 */
#if GPS_MAX_RECEIVERS > 1
void sensor_gps_update_primary(void)
{
#if defined(GPS_BLENDED_INSTANCE)
    // if blending is requested, attempt to calculate weighting for each GPS
    if ((enum GPSAutoSwitch)gps._auto_switch == GPS_BLEND) {
        gps._output_is_blended = calc_blend_weights();
        // adjust blend health counter
        if (!gps._output_is_blended) {
            gps._blend_health_counter = MIN(gps._blend_health_counter+BLEND_COUNTER_FAILURE_INCREMENT, 100);
        } else if (gps._blend_health_counter > 0) {
            gps._blend_health_counter--;
        }
        // stop blending if unhealthy
        if (gps._blend_health_counter >= 50) {
            gps._output_is_blended = false;
        }
    } else {
        gps._output_is_blended = false;
        gps._blend_health_counter = 0;
    }

    if (gps._output_is_blended) {
        // Use the weighting to calculate blended GPS states
        calc_blended_state();
        // set primary to the virtual instance
        gps.primary_instance = GPS_BLENDED_INSTANCE;
        return;
    }
#endif // defined (GPS_BLENDED_INSTANCE)

    // check the primary param is set to possible GPS
    int8_t primary_param = gps._primary;
    if ((primary_param < 0) || (primary_param>=GPS_MAX_RECEIVERS)) {
        primary_param = 0;
    }
    // if primary is not enabled try first instance
    if (sensor_gps_get_type(primary_param) == GPS_TYPE_NONE) {
        primary_param = 0;
    }

    if ((enum GPSAutoSwitch)gps._auto_switch == GPS_NONE) {
        // No switching of GPSs, always use the primary instance
        gps.primary_instance = primary_param;
        return;
    }

    uint32_t now = time_millis();

    // special handling of RTK moving baseline pair. Always use the
    // base as the rover position is derived from the base, which
    // means the rover always has worse position and velocity than the
    // base. This overrides the normal logic which would select the
    // rover as it typically is in fix type 6 (RTK) whereas base is
    // usually fix type 3
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        if (((gps._type[i] == GPS_TYPE_UBLOX_RTK_BASE) || (gps._type[i] == GPS_TYPE_UAVCAN_RTK_BASE)) &&
            ((gps._type[i^1] == GPS_TYPE_UBLOX_RTK_ROVER) || (gps._type[i^1] == GPS_TYPE_UAVCAN_RTK_ROVER)) &&
            ((gps.state[i].status >= GPS_OK_FIX_3D) || (gps.state[i].status >= gps.state[i^1].status))) {
            if (gps.primary_instance != i) {
                gps._last_instance_swap_ms = now;
                gps.primary_instance = i;
            }
            // don't do any more switching logic. We don't want the
            // RTK status of the rover to cause a switch
            return;
        }
    }

#if defined(GPS_BLENDED_INSTANCE)
    // handling switching away from blended GPS
    if (gps.primary_instance == GPS_BLENDED_INSTANCE) {
        gps.primary_instance = 0;
        for (uint8_t i=1; i<GPS_MAX_RECEIVERS; i++) {
            // choose GPS with highest state or higher number of satellites
            if ((gps.state[i].status > gps.state[gps.primary_instance].status) ||
                ((gps.state[i].status == gps.state[gps.primary_instance].status) && (gps.state[i].num_sats > gps.state[gps.primary_instance].num_sats))) {
                gps.primary_instance = i;
                gps._last_instance_swap_ms = now;
            }
        }
        return;
    }
#endif  // defined(GPS_BLENDED_INSTANCE)

    // Use primary if 3D fix or better
    if (((enum GPSAutoSwitch)gps._auto_switch == GPS_USE_PRIMARY_IF_3D_FIX) && (gps.state[primary_param].status >= GPS_OK_FIX_3D)) {
        // Primary GPS has a least a 3D fix, switch to it if necessary
        if (gps.primary_instance != primary_param) {
            gps.primary_instance = primary_param;
            gps._last_instance_swap_ms = now;
        }
        return;
    }

    // handle switch between real GPSs
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        if (i == gps.primary_instance) {
            continue;
        }
        if (gps.state[i].status > gps.state[gps.primary_instance].status) {
            // we have a higher status lock, or primary is set to the blended GPS, change GPS
            gps.primary_instance = i;
            gps._last_instance_swap_ms = now;
            continue;
        }

        bool another_gps_has_1_or_more_sats = (gps.state[i].num_sats >= gps.state[gps.primary_instance].num_sats + 1);

        if (gps.state[i].status == gps.state[gps.primary_instance].status && another_gps_has_1_or_more_sats) {

            bool another_gps_has_2_or_more_sats = (gps.state[i].num_sats >= gps.state[gps.primary_instance].num_sats + 2);

            if ((another_gps_has_1_or_more_sats && (now - gps._last_instance_swap_ms) >= 20000) ||
                (another_gps_has_2_or_more_sats && (now - gps._last_instance_swap_ms) >= 5000)) {
                // this GPS has more satellites than the
                // current primary, switch primary. Once we switch we will
                // then tend to stick to the new GPS as primary. We don't
                // want to switch too often as it will look like a
                // position shift to the controllers.
                gps.primary_instance = i;
                gps._last_instance_swap_ms = now;
            }
        }
    }
}
#endif  // GPS_MAX_RECEIVERS > 1

/*
  update one GPS instance. This should be called at 10Hz or greater
 */
void sensor_gps_update_instance(uint8_t instance)
{
    if (gps._type[instance] == GPS_TYPE_HIL) {
        // in HIL, leave info alone
        return;
    }
    if (gps._type[instance] == GPS_TYPE_NONE) {
        // not enabled
        gps.state[instance].status = NO_GPS;
        gps.state[instance].hdop = GPS_UNKNOWN_DOP;
        gps.state[instance].vdop = GPS_UNKNOWN_DOP;
        return;
    }
    if (gps.locked_ports & (1U<<instance)) {
        // the port is locked by another driver
        return;
    }

    if (gps.drivers[instance] == NULL) {
        // we don't yet know the GPS type of this one, or it has timed
        // out and needs to be re-initialised
        sensor_gps_detect_instance(instance);
        return;
    }

    if (gps._auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) {
        sensor_gps_send_blob_update(instance);
    }

    // we have an active driver for this instance
    bool result = sensor_gps_backend_read(gps.drivers[instance]);
    uint32_t tnow = time_millis();

    // if we did not get a message, and the idle timer of 2 seconds
    // has expired, re-initialise the GPS. This will cause GPS
    // detection to run again
    bool data_should_be_logged = false;
    if (!result) {
        if (tnow - gps.timing[instance].last_message_time_ms > GPS_TIMEOUT_MS) {
            rt_memset((void *)&gps.state[instance], 0, sizeof(gps.state[instance]));
            gps.state[instance].instance = instance;
            gps.state[instance].hdop = GPS_UNKNOWN_DOP;
            gps.state[instance].vdop = GPS_UNKNOWN_DOP;
            gps.timing[instance].last_message_time_ms = tnow;
            gps.timing[instance].delta_time_ms = GPS_TIMEOUT_MS;
            // do not try to detect again if type is MAV or UAVCAN
            if (gps._type[instance] == GPS_TYPE_MAV ||
                gps._type[instance] == GPS_TYPE_UAVCAN ||
                gps._type[instance] == GPS_TYPE_UAVCAN_RTK_BASE ||
                gps._type[instance] == GPS_TYPE_UAVCAN_RTK_ROVER) {
                gps.state[instance].status = NO_FIX;
            } else {
                // free the driver before we run the next detection, so we
                // don't end up with two allocated at any time
                sensor_gps_backend_destructor(gps.drivers[instance]);
                rt_free(gps.drivers[instance]);
                gps.drivers[instance] = NULL;
                gps.state[instance].status = NO_GPS;
            }
            // log this data as a "flag" that the GPS is no longer
            // valid (see PR#8144)
            data_should_be_logged = true;
        }
    } else {
        if (gps.state[instance].uart_timestamp_ms != 0) {
            // set the timestamp for this messages based on
            // set_uart_timestamp() in backend, if available
            tnow = gps.state[instance].uart_timestamp_ms;
            gps.state[instance].uart_timestamp_ms = 0;
        }
        // delta will only be correct after parsing two messages
        gps.timing[instance].delta_time_ms = tnow - gps.timing[instance].last_message_time_ms;
        gps.timing[instance].last_message_time_ms = tnow;
        // if GPS disabled for flight testing then don't update fix timing value
        if (gps.state[instance].status >= GPS_OK_FIX_2D && !gps._force_disable_gps) {
            gps.timing[instance].last_fix_time_ms = tnow;
        }

        data_should_be_logged = true;
    }

#if GPS_MAX_RECEIVERS > 1
    if (gps.drivers[instance] && gps._type[instance] == GPS_TYPE_UBLOX_RTK_BASE) {
        // see if a moving baseline base has some RTCMv3 data
        // which we need to pass along to the rover
        const uint8_t *rtcm_data;
        uint16_t rtcm_len;
        if (sensor_gps_backend_get_RTCMV3(gps.drivers[instance], rtcm_data, &rtcm_len)) {
            for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
                if (i != instance && gps._type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
                    // pass the data to the rover
                    sensor_gps_inject_data(i, rtcm_data, rtcm_len);
                    sensor_gps_backend_clear_RTCMV3(gps.drivers[instance]);
                    break;
                }
            }
        }
    }
#endif

    if (data_should_be_logged) {
        // keep count of delayed frames and average frame delay for health reporting
        const uint16_t gps_max_delta_ms = 245; // 200 ms (5Hz) + 45 ms buffer
        struct GPS_timing *t = &gps.timing[instance];

        if (t->delta_time_ms > gps_max_delta_ms) {
            t->delayed_count++;
        } else {
            t->delayed_count = 0;
        }
        if (t->delta_time_ms < 2000) {
            if (t->average_delta_ms <= 0) {
                t->average_delta_ms = t->delta_time_ms;
            } else {
                t->average_delta_ms = 0.98f * t->average_delta_ms + 0.02f * t->delta_time_ms;
            }
        }
    }

    if (gps.state[instance].status >= GPS_OK_FIX_3D) {
        const uint64_t now = sensor_gps_time_epoch_usec(instance);
        if (now != 0) {
            rtc_set_utc_usec(now, SOURCE_GPS);
        }
    }
}

/*
  run detection step for one GPS instance. If this finds a GPS then it
  will fill in drivers[instance] and change state[instance].status
  from NO_GPS to NO_FIX.
 */
void sensor_gps_detect_instance(uint8_t instance)
{
    sensor_gps_backend_t new_gps = NULL;
    struct gps_detect_state *dstate = &gps.detect_state[instance];
    const uint32_t now = time_millis();

    gps.state[instance].status = NO_GPS;
    gps.state[instance].hdop = GPS_UNKNOWN_DOP;
    gps.state[instance].vdop = GPS_UNKNOWN_DOP;

    if (gps._port[instance] == NULL) {
        // UART not available
        return;
    }

    // all remaining drivers automatically cycle through baud rates to detect
    // the correct baud rate, and should have the selected baud broadcast
    dstate->auto_detected_baud = true;

    if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) {
        // try the next baud rate
        // incrementing like this will skip the first element in array of bauds
        // this is okay, and relied upon
        dstate->current_baud++;
        if (dstate->current_baud == ARRAY_SIZE(_baudrates)) {
            dstate->current_baud = 0;
        }
        uint32_t baudrate = gps._baudrates[dstate->current_baud];
        SerialManager_set_baudrate(gps._port[instance], baudrate);
        dstate->last_baud_change_ms = now;

        if (gps._auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY && new_gps == NULL) {
            if (gps._type[instance] == GPS_TYPE_UBLOX && (gps._driver_options & UBX_Use115200)) {
                static const char blob[] = UBLOX_SET_BINARY_115200;
                sensor_gps_send_blob_start(instance, blob, sizeof(blob));
            } else {
                sensor_gps_send_blob_start(instance, gps._initialisation_blob, sizeof(gps._initialisation_blob));
            }
        }
    }

    if (gps._auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY && new_gps == NULL) {
        sensor_gps_send_blob_update(instance);
    }

    while (gps.initblob_state[instance].remaining == 0 && SerialManager_rx_available(gps._port[instance]) > 0
           && new_gps == NULL) {
        uint8_t data = SerialManager_read_byte(gps._port[instance]);
        /*
          running a uBlox at less than 38400 will lead to packet
          corruption, as we can't receive the packets in the 200ms
          window for 5Hz fixes. The NMEA startup message should force
          the uBlox into 230400 no matter what rate it is configured
          for.
        */

        if ((gps._type[instance] == GPS_TYPE_AUTO ||
             gps._type[instance] == GPS_TYPE_UBLOX) &&
            ((!gps._auto_config && gps._baudrates[dstate->current_baud] >= 38400) ||
             (gps._baudrates[dstate->current_baud] >= 115200 && (gps._driver_options & UBX_Use115200)) ||
             gps._baudrates[dstate->current_baud] == 230400) &&
            sensor_gps_ublox_detect(&dstate->ublox_detect_state, data)) {
            new_gps = sensor_gps_ublox_probe(&gps.state[instance], gps._port[instance], GPS_ROLE_NORMAL);
        }

        const uint32_t ublox_mb_required_baud = (gps._driver_options & UBX_MBUseUart2)?230400:460800;
        if ((gps._type[instance] == GPS_TYPE_UBLOX_RTK_BASE ||
             gps._type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) &&
            gps._baudrates[dstate->current_baud] == ublox_mb_required_baud &&
            sensor_gps_ublox_detect(&dstate->ublox_detect_state, data)) {
            enum GPS_Role role;
            if (gps._type[instance] == GPS_TYPE_UBLOX_RTK_BASE) {
                role = GPS_ROLE_MB_BASE;
            } else {
                role = GPS_ROLE_MB_ROVER;
            }
            new_gps = sensor_gps_ublox_probe(&gps.state[instance], gps._port[instance], role);
        }
        if (new_gps != NULL) {
            goto found_gps;
        }
    }

found_gps:
    if (new_gps != NULL) {
        gps.state[instance].status = NO_FIX;
        gps.drivers[instance] = new_gps;
        gps.timing[instance].last_message_time_ms = now;
        gps.timing[instance].delta_time_ms = GPS_TIMEOUT_MS;
        sensor_gps_backend_broadcast_gps_type(new_gps);
    }
}

void sensor_gps_inject_data(uint8_t instance, const uint8_t *data, uint16_t len)
{
    if (instance < GPS_MAX_RECEIVERS && gps.drivers[instance] != NULL) {
        sensor_gps_backend_inject_data(gps.drivers[instance], data, len);
    }
}

/**
   calculate current time since the unix epoch in microseconds
 */
uint64_t sensor_gps_time_epoch_usec(uint8_t instance)
{
    const struct GPS_State *istate = &gps.state[instance];
    if (istate->last_gps_time_ms == 0 || istate->time_week == 0) {
        return 0;
    }
    uint64_t fix_time_ms = sensor_gps_time_epoch_convert(istate->time_week, istate->time_week_ms);
    // add in the milliseconds since the last fix
    return (fix_time_ms + (time_millis() - istate->last_gps_time_ms)) * 1000ULL;
}

/**
   convert GPS week and milliseconds to unix epoch in milliseconds
 */
uint64_t sensor_gps_time_epoch_convert(uint16_t gps_week, uint32_t gps_ms)
{
    uint64_t fix_time_ms = UNIX_OFFSET_MSEC + gps_week * AP_MSEC_PER_WEEK + gps_ms;
    return fix_time_ms;
}

bool sensor_gps_is_healthy(uint8_t instance)
{
    if (instance >= GPS_MAX_INSTANCES) {
        return false;
    }

    if (sensor_gps_get_type(gps._primary) == GPS_TYPE_NONE) {
        return false;
    }

    /*
      allow two lost frames before declaring the GPS unhealthy, but
      require the average frame rate to be close to 5Hz. We allow for
      a bit higher average for a rover due to the packet loss that
      happens with the RTCMv3 data
     */
    const uint8_t delay_threshold = 2;
    const float delay_avg_max = ((gps._type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) || (gps._type[instance] == GPS_TYPE_UAVCAN_RTK_ROVER))?245:215;
    const struct GPS_timing *t = &gps.timing[instance];
    bool delay_ok = (t->delayed_count < delay_threshold) &&
        t->average_delta_ms < delay_avg_max &&
        gps.state[instance].lagged_sample_count < 5;

#if defined(GPS_BLENDED_INSTANCE)
    if (instance == GPS_BLENDED_INSTANCE) {
        return delay_ok && sensor_gps_blend_health_check();
    }
#endif

    return delay_ok && gps.drivers[instance] != NULL &&
           sensor_gps_backend_is_healthy(gps.drivers[instance]);;
}

// pre-arm check of GPS blending.  True means healthy or that blending is not being used
bool sensor_gps_blend_health_check()
{
    return (gps._blend_health_counter < 50);
}

// return number of active GPS sensors. Note that if the first GPS
// is not present but the 2nd is then we return 2. Note that a blended
// GPS solution is treated as an additional sensor.
uint8_t sensor_gps_num_sensors(void)
{
    if (!gps._output_is_blended) {
        return gps.num_instances;
    } else {
        return gps.num_instances+1;
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
struct GPS_State* sensor_gps_primary_state()
{
    return &gps.state[gps.primary_instance];
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
struct GPS_timing* sensor_gps_primary_timing()
{
    return &gps.timing[gps.primary_instance];
}

bool sensor_gps_backends_healthy(char failure_msg[50], uint16_t failure_msg_len)
{
    for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
        if (gps._type[i] == GPS_TYPE_UBLOX_RTK_ROVER ||
            gps._type[i] == GPS_TYPE_UAVCAN_RTK_ROVER) {
            if (time_millis() - gps.state[i].gps_yaw_time_ms > 15000) {
                snprintf(failure_msg, failure_msg_len, "GPS[%u] yaw not available", (unsigned)(i+1));
                return false;
            }
        }
    }
    return true;
}

/// Query GPS status
enum GPS_Status sensor_gps_status2(uint8_t instance)
{
    if (gps._force_disable_gps && gps.state[instance].status > NO_FIX) {
        return NO_FIX;
    }
    return gps.state[instance].status;
}

enum GPS_Status sensor_gps_status(void)
{
    return sensor_gps_status2(gps.primary_instance);
}

// pre-arm check that all GPSs are close to each other.  farthest distance between GPSs (in meters) is returned
bool sensor_gps_all_consistent(float* distance)
{
    // return true immediately if only one valid receiver
    if (gps.num_instances <= 1 ||
        gps.drivers[0] == NULL || gps._type[0] == GPS_TYPE_NONE) {
        *distance = 0;
        return true;
    }

    // calculate distance
    Vector3f_t v = location_get_distance_ned(&gps.state[0].location, &gps.state[1].location);
    *distance = vec3_length(&v);

    // success if distance is within 50m
    return (*distance < 50);
}

// location of last fix
const Location *sensor_gps_location2(uint8_t instance)
{
    return &gps.state[instance].location;
}

const Location *sensor_gps_location()
{
    return &gps.state[gps.primary_instance].location;
}

void sensor_gps_broadcast_first_configuration_failure_reason(void)
{
    uint8_t unconfigured;
    if (sensor_gps_first_unconfigured_gps(&unconfigured)) {
        if (gps.drivers[unconfigured] == NULL) {
            mavproxy_send_statustext(MAV_SEVERITY_INFO, "GPS %d: was not found", unconfigured + 1);
        } else {
            sensor_gps_backend_broadcast_configuration_failure_reason(gps.drivers[unconfigured]);
        }
    }
}

// horizontal dilution of precision
uint16_t sensor_gps_get_hdop2(uint8_t instance)
{
    return gps.state[instance].hdop;
}

uint16_t sensor_gps_get_hdop()
{
    return gps.state[gps.primary_instance].hdop;
}

/// 
// return true if a specific type of GPS uses a UART
static bool needs_uart(enum GPS_Type type)
{
    switch (type) {
    case GPS_TYPE_NONE:
    case GPS_TYPE_HIL:
    case GPS_TYPE_UAVCAN:
    case GPS_TYPE_UAVCAN_RTK_BASE:
    case GPS_TYPE_UAVCAN_RTK_ROVER:
    case GPS_TYPE_MAV:
    case GPS_TYPE_MSP:
    case GPS_TYPE_EXTERNAL_AHRS:
        return false;
    default:
        break;
    }
    return true;
}

#if defined(GPS_BLENDED_INSTANCE)
/*
 calculate the weightings used to blend GPSs location and velocity data
*/
static bool calc_blend_weights(void)
{
    // zero the blend weights
    rt_memset(&gps._blend_weights[0], 0, sizeof(gps._blend_weights));


    _Static_assert(GPS_MAX_RECEIVERS == 2, "GPS blending only currently works with 2 receivers");
    // Note that the early quit below relies upon exactly 2 instances
    // The time delta calculations below also rely upon every instance being currently detected and being parsed

    // exit immediately if not enough receivers to do blending
    if (gps.state[0].status <= NO_FIX || gps.state[1].status <= NO_FIX) {
        return false;
    }

    // Use the oldest non-zero time, but if time difference is excessive, use newest to prevent a disconnected receiver from blocking updates
    uint32_t max_ms = 0; // newest non-zero system time of arrival of a GPS message
    uint32_t min_ms = -1; // oldest non-zero system time of arrival of a GPS message
    uint32_t max_rate_ms = 0; // largest update interval of a GPS receiver
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        // Find largest and smallest times
        if (gps.state[i].last_gps_time_ms > max_ms) {
            max_ms = gps.state[i].last_gps_time_ms;
        }
        if ((gps.state[i].last_gps_time_ms < min_ms) && (gps.state[i].last_gps_time_ms > 0)) {
            min_ms = gps.state[i].last_gps_time_ms;
        }
        max_rate_ms = MAX(get_rate_ms(i), max_rate_ms);
        if (isinf(gps.state[i].speed_accuracy) ||
            isinf(gps.state[i].horizontal_accuracy) ||
            isinf(gps.state[i].vertical_accuracy)) {
            return false;
        }
    }
    if ((max_ms - min_ms) < (2 * max_rate_ms)) {
        // data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated
        gps.state[GPS_BLENDED_INSTANCE].last_gps_time_ms = min_ms;
    } else {
        // receiver data has timed out so fail out of blending
        return false;
    }

    // calculate the sum squared speed accuracy across all GPS sensors
    float speed_accuracy_sum_sq = 0.0f;
    if (gps._blend_mask & BLEND_MASK_USE_SPD_ACC) {
        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
            if (gps.state[i].status >= GPS_OK_FIX_3D) {
                if (gps.state[i].have_speed_accuracy && gps.state[i].speed_accuracy > 0.0f) {
                    speed_accuracy_sum_sq += gps.state[i].speed_accuracy * gps.state[i].speed_accuracy;
                } else {
                    // not all receivers support this metric so set it to zero and don't use it
                    speed_accuracy_sum_sq = 0.0f;
                    break;
                }
            }
        }
    }

    // calculate the sum squared horizontal position accuracy across all GPS sensors
    float horizontal_accuracy_sum_sq = 0.0f;
    if (gps._blend_mask & BLEND_MASK_USE_HPOS_ACC) {
        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
            if (gps.state[i].status >= GPS_OK_FIX_2D) {
                if (gps.state[i].have_horizontal_accuracy && gps.state[i].horizontal_accuracy > 0.0f) {
                    horizontal_accuracy_sum_sq += gps.state[i].horizontal_accuracy * gps.state[i].horizontal_accuracy;
                } else {
                    // not all receivers support this metric so set it to zero and don't use it
                    horizontal_accuracy_sum_sq = 0.0f;
                    break;
                }
            }
        }
    }

    // calculate the sum squared vertical position accuracy across all GPS sensors
    float vertical_accuracy_sum_sq = 0.0f;
    if (gps._blend_mask & BLEND_MASK_USE_VPOS_ACC) {
        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
            if (gps.state[i].status >= GPS_OK_FIX_3D) {
                if (gps.state[i].have_vertical_accuracy && gps.state[i].vertical_accuracy > 0.0f) {
                    vertical_accuracy_sum_sq += gps.state[i].vertical_accuracy * gps.state[i].vertical_accuracy;
                } else {
                    // not all receivers support this metric so set it to zero and don't use it
                    vertical_accuracy_sum_sq = 0.0f;
                    break;
                }
            }
        }
    }
    // Check if we can do blending using reported accuracy
    bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f || speed_accuracy_sum_sq > 0.0f);

    // if we can't do blending using reported accuracy, return false and hard switch logic will be used instead
    if (!can_do_blending) {
        return false;
    }

    float sum_of_all_weights = 0.0f;

    // calculate a weighting using the reported horizontal position
    float hpos_blend_weights[GPS_MAX_RECEIVERS] = {};
    if (horizontal_accuracy_sum_sq > 0.0f && (gps._blend_mask & BLEND_MASK_USE_HPOS_ACC)) {
        // calculate the weights using the inverse of the variances
        float sum_of_hpos_weights = 0.0f;
        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
            if (gps.state[i].status >= GPS_OK_FIX_2D && gps.state[i].horizontal_accuracy >= 0.001f) {
                hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (gps.state[i].horizontal_accuracy * gps.state[i].horizontal_accuracy);
                sum_of_hpos_weights += hpos_blend_weights[i];
            }
        }
        // normalise the weights
        if (sum_of_hpos_weights > 0.0f) {
            for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
                hpos_blend_weights[i] = hpos_blend_weights[i] / sum_of_hpos_weights;
            }
            sum_of_all_weights += 1.0f;
        }
    }

    // calculate a weighting using the reported vertical position accuracy
    float vpos_blend_weights[GPS_MAX_RECEIVERS] = {};
    if (vertical_accuracy_sum_sq > 0.0f && (gps._blend_mask & BLEND_MASK_USE_VPOS_ACC)) {
        // calculate the weights using the inverse of the variances
        float sum_of_vpos_weights = 0.0f;
        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
            if (gps.state[i].status >= GPS_OK_FIX_3D && gps.state[i].vertical_accuracy >= 0.001f) {
                vpos_blend_weights[i] = vertical_accuracy_sum_sq / (gps.state[i].vertical_accuracy * gps.state[i].vertical_accuracy);
                sum_of_vpos_weights += vpos_blend_weights[i];
            }
        }
        // normalise the weights
        if (sum_of_vpos_weights > 0.0f) {
            for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
                vpos_blend_weights[i] = vpos_blend_weights[i] / sum_of_vpos_weights;
            }
            sum_of_all_weights += 1.0f;
        };
    }

    // calculate a weighting using the reported speed accuracy
    float spd_blend_weights[GPS_MAX_RECEIVERS] = {};
    if (speed_accuracy_sum_sq > 0.0f && (gps._blend_mask & BLEND_MASK_USE_SPD_ACC)) {
        // calculate the weights using the inverse of the variances
        float sum_of_spd_weights = 0.0f;
        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
            if (gps.state[i].status >= GPS_OK_FIX_3D && gps.state[i].speed_accuracy >= 0.001f) {
                spd_blend_weights[i] = speed_accuracy_sum_sq / (gps.state[i].speed_accuracy * gps.state[i].speed_accuracy);
                sum_of_spd_weights += spd_blend_weights[i];
            }
        }
        // normalise the weights
        if (sum_of_spd_weights > 0.0f) {
            for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
                spd_blend_weights[i] = spd_blend_weights[i] / sum_of_spd_weights;
            }
            sum_of_all_weights += 1.0f;
        }
    }

    if (!math_flt_positive(sum_of_all_weights)) {
        return false;
    }

    // calculate an overall weight
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        gps._blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights;
    }

    return true;
}

/*
 calculate a blended GPS state
*/
static void calc_blended_state(void)
{
    // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver
    gps.state[GPS_BLENDED_INSTANCE].instance = GPS_BLENDED_INSTANCE;
    gps.state[GPS_BLENDED_INSTANCE].status = NO_FIX;
    gps.state[GPS_BLENDED_INSTANCE].time_week_ms = 0;
    gps.state[GPS_BLENDED_INSTANCE].time_week = 0;
    gps.state[GPS_BLENDED_INSTANCE].ground_speed = 0.0f;
    gps.state[GPS_BLENDED_INSTANCE].ground_course = 0.0f;
    gps.state[GPS_BLENDED_INSTANCE].hdop = GPS_UNKNOWN_DOP;
    gps.state[GPS_BLENDED_INSTANCE].vdop = GPS_UNKNOWN_DOP;
    gps.state[GPS_BLENDED_INSTANCE].num_sats = 0;
    vec3_zero(&gps.state[GPS_BLENDED_INSTANCE].velocity);
    gps.state[GPS_BLENDED_INSTANCE].speed_accuracy = 1e6f;
    gps.state[GPS_BLENDED_INSTANCE].horizontal_accuracy = 1e6f;
    gps.state[GPS_BLENDED_INSTANCE].vertical_accuracy = 1e6f;
    gps.state[GPS_BLENDED_INSTANCE].have_vertical_velocity = false;
    gps.state[GPS_BLENDED_INSTANCE].have_speed_accuracy = false;
    gps.state[GPS_BLENDED_INSTANCE].have_horizontal_accuracy = false;
    gps.state[GPS_BLENDED_INSTANCE].have_vertical_accuracy = false;
    location_zero(&gps.state[GPS_BLENDED_INSTANCE].location);

    vec3_zero(&gps._blended_antenna_offset);
    gps._blended_lag_sec = 0;

    const uint32_t last_blended_message_time_ms = gps.timing[GPS_BLENDED_INSTANCE].last_message_time_ms;

    gps.timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = 0;
    gps.timing[GPS_BLENDED_INSTANCE].last_message_time_ms = 0;

    Vector3f_t velocity_tmp_m;
    // combine the states into a blended solution
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        // use the highest status
        if (gps.state[i].status > gps.state[GPS_BLENDED_INSTANCE].status) {
            gps.state[GPS_BLENDED_INSTANCE].status = gps.state[i].status;
        }

        // calculate a blended average velocity
        vec3_mult(&velocity_tmp_m, &gps.state[i].velocity, gps._blend_weights[i]);
        vec3_add(&gps.state[GPS_BLENDED_INSTANCE].velocity, &gps.state[GPS_BLENDED_INSTANCE].velocity, &velocity_tmp_m);

        // report the best valid accuracies and DOP metrics

        if (gps.state[i].have_horizontal_accuracy && gps.state[i].horizontal_accuracy > 0.0f && gps.state[i].horizontal_accuracy < gps.state[GPS_BLENDED_INSTANCE].horizontal_accuracy) {
            gps.state[GPS_BLENDED_INSTANCE].have_horizontal_accuracy = true;
            gps.state[GPS_BLENDED_INSTANCE].horizontal_accuracy = gps.state[i].horizontal_accuracy;
        }

        if (gps.state[i].have_vertical_accuracy && gps.state[i].vertical_accuracy > 0.0f && gps.state[i].vertical_accuracy < gps.state[GPS_BLENDED_INSTANCE].vertical_accuracy) {
            gps.state[GPS_BLENDED_INSTANCE].have_vertical_accuracy = true;
            gps.state[GPS_BLENDED_INSTANCE].vertical_accuracy = gps.state[i].vertical_accuracy;
        }

        if (gps.state[i].have_vertical_velocity) {
            gps.state[GPS_BLENDED_INSTANCE].have_vertical_velocity = true;
        }

        if (gps.state[i].have_speed_accuracy && gps.state[i].speed_accuracy > 0.0f && gps.state[i].speed_accuracy < gps.state[GPS_BLENDED_INSTANCE].speed_accuracy) {
            gps.state[GPS_BLENDED_INSTANCE].have_speed_accuracy = true;
            gps.state[GPS_BLENDED_INSTANCE].speed_accuracy = gps.state[i].speed_accuracy;
        }

        if (gps.state[i].hdop > 0 && gps.state[i].hdop < gps.state[GPS_BLENDED_INSTANCE].hdop) {
            gps.state[GPS_BLENDED_INSTANCE].hdop = gps.state[i].hdop;
        }

        if (gps.state[i].vdop > 0 && gps.state[i].vdop < gps.state[GPS_BLENDED_INSTANCE].vdop) {
            gps.state[GPS_BLENDED_INSTANCE].vdop = gps.state[i].vdop;
        }

        if (gps.state[i].num_sats > 0 && gps.state[i].num_sats > gps.state[GPS_BLENDED_INSTANCE].num_sats) {
            gps.state[GPS_BLENDED_INSTANCE].num_sats = gps.state[i].num_sats;
        }

        // report a blended average GPS antenna position
        Vector3f_t temp_antenna_offset = gps._antenna_offset[i];
        vec3_mult(&temp_antenna_offset, &temp_antenna_offset, gps._blend_weights[i]);
        vec3_add(&gps._blended_antenna_offset, &gps._blended_antenna_offset, &temp_antenna_offset);

        // blend the timing data
        if (gps.timing[i].last_fix_time_ms > gps.timing[GPS_BLENDED_INSTANCE].last_fix_time_ms) {
            gps.timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = gps.timing[i].last_fix_time_ms;
        }
        if (gps.timing[i].last_message_time_ms > gps.timing[GPS_BLENDED_INSTANCE].last_message_time_ms) {
            gps.timing[GPS_BLENDED_INSTANCE].last_message_time_ms = gps.timing[i].last_message_time_ms;
        }
    }

    /*
     * Calculate an instantaneous weighted/blended average location from the available GPS instances and store in the _output_state.
     * This will be statistically the most likely location, but will be not stable enough for direct use by the autopilot.
    */

    // Use the GPS with the highest weighting as the reference position
    float best_weight = 0.0f;
    uint8_t best_index = 0;
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        if (gps._blend_weights[i] > best_weight) {
            best_weight = gps._blend_weights[i];
            best_index = i;
            gps.state[GPS_BLENDED_INSTANCE].location = gps.state[i].location;
        }
    }

    // Calculate the weighted sum of horizontal and vertical position offsets relative to the reference position
    Vector2f_t blended_NE_offset_m;
    Vector2f_t blended_NE_offset_tmp_m;
    float blended_alt_offset_cm = 0.0f;
    vec2_zero(&blended_NE_offset_m);
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        if (gps._blend_weights[i] > 0.0f && i != best_index) {
            blended_alt_offset_cm += (float)(gps.state[i].location.alt - gps.state[GPS_BLENDED_INSTANCE].location.alt) * gps._blend_weights[i];

            blended_NE_offset_tmp_m = location_get_distance_ne(&gps.state[GPS_BLENDED_INSTANCE].location, &gps.state[i].location);
            vec2_mult(&blended_NE_offset_tmp_m, &blended_NE_offset_tmp_m, gps._blend_weights[i]);
            vec2_add(&blended_NE_offset_m, &blended_NE_offset_m, &blended_NE_offset_tmp_m);
        }
    }

    // Add the sum of weighted offsets to the reference location to obtain the blended location
    location_offset(&gps.state[GPS_BLENDED_INSTANCE].location, blended_NE_offset_m.x, blended_NE_offset_m.y);
    gps.state[GPS_BLENDED_INSTANCE].location.alt += (int)blended_alt_offset_cm;

    // Calculate ground speed and course from blended velocity vector
    gps.state[GPS_BLENDED_INSTANCE].ground_speed = vec3_length_xy(&gps.state[GPS_BLENDED_INSTANCE].velocity);
    gps.state[GPS_BLENDED_INSTANCE].ground_course = math_wrap_360(degrees(atan2f(gps.state[GPS_BLENDED_INSTANCE].velocity.y, gps.state[GPS_BLENDED_INSTANCE].velocity.x)));

    // If the GPS week is the same then use a blended time_week_ms
    // If week is different, then use time stamp from GPS with largest weighting
    // detect inconsistent week data
    uint8_t last_week_instance = 0;
    bool weeks_consistent = true;
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        if (last_week_instance == 0 && gps._blend_weights[i] > 0) {
            // this is our first valid sensor week data
            last_week_instance = gps.state[i].time_week;
        } else if (last_week_instance != 0 && gps._blend_weights[i] > 0 && last_week_instance != gps.state[i].time_week) {
            // there is valid sensor week data that is inconsistent
            weeks_consistent = false;
        }
    }
    // calculate output
    if (!weeks_consistent) {
        // use data from highest weighted sensor
        gps.state[GPS_BLENDED_INSTANCE].time_week = gps.state[best_index].time_week;
        gps.state[GPS_BLENDED_INSTANCE].time_week_ms = gps.state[best_index].time_week_ms;
    } else {
        // use week number from highest weighting GPS (they should all have the same week number)
        gps.state[GPS_BLENDED_INSTANCE].time_week = gps.state[best_index].time_week;
        // calculate a blended value for the number of ms lapsed in the week
        double temp_time_0 = 0.0;
        for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
            if (gps._blend_weights[i] > 0.0f) {
                temp_time_0 += (double)gps.state[i].time_week_ms * (double)gps._blend_weights[i];
            }
        }
        gps.state[GPS_BLENDED_INSTANCE].time_week_ms = (uint32_t)temp_time_0;
    }

    // calculate a blended value for the timing data and lag
    double temp_time_1 = 0.0;
    double temp_time_2 = 0.0;
    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
        if (gps._blend_weights[i] > 0.0f) {
            temp_time_1 += (double)gps.timing[i].last_fix_time_ms * (double)gps._blend_weights[i];
            temp_time_2 += (double)gps.timing[i].last_message_time_ms * (double)gps._blend_weights[i];
            float gps_lag_sec = 0;
            sensor_gps_get_lag(i, &gps_lag_sec);
            gps._blended_lag_sec += gps_lag_sec * gps._blend_weights[i];
        }
    }
    gps.timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = (uint32_t)temp_time_1;
    gps.timing[GPS_BLENDED_INSTANCE].last_message_time_ms = (uint32_t)temp_time_2;

}
#endif // GPS_BLENDED_INSTANCE

/*
  returns the desired gps update rate in milliseconds
  this does not provide any guarantee that the GPS is updating at the requested
  rate it is simply a helper for use in the backends for determining what rate
  they should be configuring the GPS to run at
*/
static uint16_t get_rate_ms(uint8_t instance)
{
    // sanity check
    if (instance >= gps.num_instances || gps._rate_ms[instance] <= 0) {
        return GPS_MAX_RATE_MS;
    }
    return MIN(gps._rate_ms[instance], GPS_MAX_RATE_MS);
}

static void publish_sensor_gps()
{
    static uitc_sensor_gps sensor_gps = {0};
    static uint32_t primary_gps_ms = 0;

    if (sensor_gps_is_healthy(gps.primary_instance) && gps.timing[gps.primary_instance].last_message_time_ms - primary_gps_ms > 50) {
        primary_gps_ms = gps.timing[gps.primary_instance].last_message_time_ms;

        sensor_gps.time_gps_usec = sensor_gps_time_epoch_usec(gps.primary_instance);
        sensor_gps.timestamp_us = (uint64_t)gps.state[gps.primary_instance].last_gps_time_ms * 1000;
        sensor_gps.lat = gps.state[gps.primary_instance].location.lat;
        sensor_gps.lon = gps.state[gps.primary_instance].location.lng;
        sensor_gps.alt_msl = gps.state[gps.primary_instance].location.alt * 10;
        sensor_gps.hdop = gps.state[gps.primary_instance].hdop;
        sensor_gps.vdop = gps.state[gps.primary_instance].vdop;
        sensor_gps.horizontal_accuracy = gps.state[gps.primary_instance].horizontal_accuracy;
        sensor_gps.vertical_accuracy = gps.state[gps.primary_instance].vertical_accuracy;
        sensor_gps.vel_m_s = gps.state[gps.primary_instance].ground_speed;
        sensor_gps.vel_n_m_s = gps.state[gps.primary_instance].velocity.x;
        sensor_gps.vel_e_m_s = gps.state[gps.primary_instance].velocity.y;
        sensor_gps.vel_d_m_s = gps.state[gps.primary_instance].velocity.z;
        sensor_gps.vel_ned_valid = gps.state[gps.primary_instance].have_speed_accuracy;
        sensor_gps.fix_type = gps.state[gps.primary_instance].status;
        sensor_gps.num_sats = gps.state[gps.primary_instance].num_sats;
        sensor_gps.have_gps_yaw = gps.state[gps.primary_instance].have_gps_yaw;
        sensor_gps.have_gps_yaw_accuracy = gps.state[gps.primary_instance].have_gps_yaw_accuracy;
        sensor_gps.gps_yaw_configured    = gps.state[gps.primary_instance].gps_yaw_configured;
        sensor_gps.gps_yaw = gps.state[gps.primary_instance].gps_yaw;
        sensor_gps.gps_yaw_accuracy = gps.state[gps.primary_instance].gps_yaw_accuracy;
        sensor_gps.cog_rad = math_wrap_PI(radians(gps.state[gps.primary_instance].ground_course));

        itc_publish(ITC_ID(sensor_gps), &sensor_gps);
    }
}
/*------------------------------------test------------------------------------*/


